#include <gtest/gtest.h>
#include <memory>
#include <Eigen/Core>
#include "../include/PBAFactors.h"
#include "../include/PBABlocks.hpp"
// #include "../include/ceres/LandMarkBlock.h"
#include "../../util/generateRandom.h"

using namespace Estimator;

TEST(Jacobian4Nodes_TEST /*test_case_name*/, Jacobian_POSE1 /*test_name*/)
{
    Eigen::Vector3d measurement = Eigen::Vector3d::Random();
    Eigen::Matrix3d information = Eigen::Matrix3d::Identity();

    std::shared_ptr<CeresPBAFactor4Nodes> pFactorNodes(new Estimator::CeresPBAFactor4Nodes(measurement, information));

    // prepare the parameters and jacobian for test
    double **parameters, **jacobians;
    parameters = new double *[pFactorNodes->parameterBlocks()];
    jacobians = new double *[pFactorNodes->parameterBlocks()];  

    Eigen::Vector3d residual;

    Eigen::Matrix<double, 3, 4, RowMajor> _transform1;
    _transform1.topLeftCorner<3,3>() = generateRandomRotation(3.0);
    _transform1.topRightCorner<3,1>() = Eigen::Vector3d::Random();
    PoseBlock pose1(0, _transform1);
    Eigen::Matrix<double, 3, 12, RowMajor> _jacobian1 = Eigen::Matrix<double,3,12>::Constant(std::nan(""));

    Eigen::Matrix<double, 3, 4, RowMajor> _transform2;
    _transform2.topLeftCorner<3,3>() = generateRandomRotation(3.0);
    _transform2.topRightCorner<3,1>() = Eigen::Vector3d::Random();
    PoseBlock pose2(1, _transform2);
    Eigen::Matrix<double, 3, 12, RowMajor> _jacobian2 = Eigen::Matrix<double,3,12>::Constant(std::nan(""));

     Eigen::Matrix<double, 3, 4, RowMajor> _transform3;
    _transform2.topLeftCorner<3,3>() = generateRandomRotation(3.0);
    _transform2.topRightCorner<3,1>() = Eigen::Vector3d::Random();
    PoseBlock pose3(2, _transform3);
    Eigen::Matrix<double, 3, 12, RowMajor> _jacobian3 = Eigen::Matrix<double,3,12>::Constant(std::nan(""));

    Eigen::Matrix<double, 5,1 > _landmark;
    _landmark.segment<2>(0) = Eigen::Vector2d::Random().normalized();
    _landmark.segment<3>(2) = Eigen::Vector3d::Random().normalized();
    LandMarkBlock landmark(1, _landmark);
    Eigen::Matrix<double, 3,5, RowMajor> _jacobian4 = Eigen::Matrix<double,3,5>::Constant(std::nan(""));
    // 
    parameters[0] = pose1.parameters();
    parameters[1] = pose2.parameters();
    parameters[2] = pose3.parameters();
    parameters[3] = landmark.parameters();

    jacobians[0] = _jacobian1.data();
    jacobians[1] = _jacobian2.data();
    jacobians[2] = _jacobian3.data();
    jacobians[3] = _jacobian4.data();
    
    EXPECT_NE(_transform1, _transform2);
    EXPECT_NE(_transform2, _transform3);
    // now we have random values for 3 edges...
    
    const double delta = 1e-06;
    const double relTol = 1e-6;
    // Our jacobians...

    pFactorNodes->Evaluate(parameters, residual.data(), jacobians);

    Eigen::Matrix<double, 3, 12, RowMajor> jacobian_delta;
    jacobian_delta.setZero();
    for (size_t i = 0; i < 6; ++i)
    {
        Eigen::Matrix<double, 6, 1> delta_pose = Eigen::Matrix<double, 6, 1>::Zero();
        delta_pose(i,0) = delta;
        Eigen::Matrix<double, 3, 4, RowMajor> _transform1_plus;
        PoseLocalParameterization pose_plus;
        pose_plus.Plus(pose1.parameters(), delta_pose.data(), _transform1_plus.data());
        EXPECT_NE(_transform1, _transform1_plus);
        
        Eigen::Vector3d residual_delta;
        PoseBlock pose1_plus(0, _transform1_plus);
        parameters[0] = pose1_plus.parameters();
        pFactorNodes->Evaluate(parameters, residual_delta.data(), NULL);
        jacobian_delta.col(i) = (residual_delta - residual) / delta;
    }

    Eigen::MatrixXd J_diff = jacobian_delta - _jacobian1;
    double maxDiff = std::max(-J_diff.minCoeff(), J_diff.maxCoeff());

    EXPECT_LE(maxDiff, relTol);
    delete[] jacobians;
}


TEST(Jacobian4Nodes_TEST /*test_case_name*/, Jacobian_POSE2 /*test_name*/)
{
    Eigen::Vector3d measurement = Eigen::Vector3d::Random();
    Eigen::Matrix3d information = Eigen::Matrix3d::Identity();

    std::shared_ptr<CeresPBAFactor4Nodes> pFactorNodes(new Estimator::CeresPBAFactor4Nodes(measurement, information));

    // prepare the parameters and jacobian for test
    double **parameters, **jacobians;
    parameters = new double *[pFactorNodes->parameterBlocks()];
    jacobians = new double *[pFactorNodes->parameterBlocks()];

    Eigen::Vector3d residual;

    Eigen::Matrix<double, 3, 4, RowMajor> _transform1;
    _transform1.topLeftCorner<3,3>() = generateRandomRotation(3.0);
    _transform1.topRightCorner<3,1>() = Eigen::Vector3d::Random();
    PoseBlock pose1(0, _transform1);
    Eigen::Matrix<double, 3, 12, RowMajor> _jacobian1 = Eigen::Matrix<double,3,12>::Constant(std::nan(""));

    Eigen::Matrix<double, 3, 4, RowMajor> _transform2;
    _transform2.topLeftCorner<3,3>() = generateRandomRotation(3.0);
    _transform2.topRightCorner<3,1>() = Eigen::Vector3d::Random();
    PoseBlock pose2(1, _transform2);
    Eigen::Matrix<double, 3, 12, RowMajor> _jacobian2 = Eigen::Matrix<double,3,12>::Constant(std::nan(""));

     Eigen::Matrix<double, 3, 4, RowMajor> _transform3;
    _transform3.topLeftCorner<3,3>() = generateRandomRotation(3.0);
    _transform3.topRightCorner<3,1>() = Eigen::Vector3d::Random();
    PoseBlock pose3(2, _transform3);
    Eigen::Matrix<double, 3, 12, RowMajor> _jacobian3 = Eigen::Matrix<double,3,12>::Constant(std::nan(""));

    Eigen::Matrix<double, 5,1 > _landmark;
    _landmark.segment<2>(0) = Eigen::Vector2d::Random().normalized();
    _landmark.segment<3>(2) = Eigen::Vector3d::Random().normalized();
    LandMarkBlock landmark(1, _landmark);
    Eigen::Matrix<double, 3,5, RowMajor> _jacobian4 = Eigen::Matrix<double,3,5>::Constant(std::nan(""));
    // 
    parameters[0] = pose1.parameters();
    parameters[1] = pose2.parameters();
    parameters[2] = pose3.parameters();
    parameters[3] = landmark.parameters();

    jacobians[0] = _jacobian1.data();
    jacobians[1] = _jacobian2.data();
    jacobians[2] = _jacobian3.data();
    jacobians[3] = _jacobian4.data();
    
    EXPECT_NE(_transform1, _transform2);
    EXPECT_NE(_transform2, _transform3);
    // now we have random values for 3 edges...
    
    const double delta = 1e-08;
    const double relTol = 1e-6;
    // Our jacobians...

    pFactorNodes->Evaluate(parameters, residual.data(), jacobians);

    Eigen::Matrix<double, 3, 12, RowMajor> jacobian_delta;
    jacobian_delta.setZero();
    for (size_t i = 0; i < 6; ++i)
    {
        Eigen::Matrix<double, 6, 1> delta_pose = Eigen::Matrix<double, 6, 1>::Zero();
        delta_pose(i,0) = delta;
        Eigen::Matrix<double, 3, 4, RowMajor> _transform2_plus;
        PoseLocalParameterization pose_plus;
        pose_plus.Plus(pose2.parameters(), delta_pose.data(), _transform2_plus.data());
        EXPECT_NE(_transform2, _transform2_plus);
        
        Eigen::Vector3d residual_delta;
        PoseBlock pose2_plus(0, _transform2_plus);
        parameters[1] = pose2_plus.parameters();
        pFactorNodes->Evaluate(parameters, residual_delta.data(), NULL);
        jacobian_delta.col(i) = (residual_delta - residual) / delta;
    }

    Eigen::MatrixXd J_diff = jacobian_delta - _jacobian2;
    double maxDiff = std::max(-J_diff.minCoeff(), J_diff.maxCoeff());

    EXPECT_LE(maxDiff, relTol);
    delete[] jacobians;
}


TEST(Jacobian4Nodes_TEST /*test_case_name*/, Jacobian_POSE3 /*test_name*/)
{
    Eigen::Vector3d measurement = Eigen::Vector3d::Random();
    Eigen::Matrix3d information = Eigen::Matrix3d::Identity();

    std::shared_ptr<CeresPBAFactor4Nodes> pFactorNodes(new Estimator::CeresPBAFactor4Nodes(measurement, information));

    // prepare the parameters and jacobian for test
    double **parameters, **jacobians;
    parameters = new double *[pFactorNodes->parameterBlocks()];
    jacobians = new double *[pFactorNodes->parameterBlocks()];

    Eigen::Vector3d residual;

    Eigen::Matrix<double, 3, 4, RowMajor> _transform1;
    _transform1.topLeftCorner<3,3>() = generateRandomRotation(3.0);
    _transform1.topRightCorner<3,1>() = Eigen::Vector3d::Random();
    PoseBlock pose1(0, _transform1);
    Eigen::Matrix<double, 3, 12, RowMajor> _jacobian1 = Eigen::Matrix<double,3,12>::Constant(std::nan(""));

    Eigen::Matrix<double, 3, 4, RowMajor> _transform2;
    _transform2.topLeftCorner<3,3>() = generateRandomRotation(3.0);
    _transform2.topRightCorner<3,1>() = Eigen::Vector3d::Random();
    PoseBlock pose2(1, _transform2);
    Eigen::Matrix<double, 3, 12, RowMajor> _jacobian2 = Eigen::Matrix<double,3,12>::Constant(std::nan(""));

     Eigen::Matrix<double, 3, 4, RowMajor> _transform3;
    _transform3.topLeftCorner<3,3>() = generateRandomRotation(3.0);
    _transform3.topRightCorner<3,1>() = Eigen::Vector3d::Random();
    PoseBlock pose3(2, _transform3);
    Eigen::Matrix<double, 3, 12, RowMajor> _jacobian3 = Eigen::Matrix<double,3,12>::Constant(std::nan(""));

    Eigen::Matrix<double, 5,1 > _landmark;
    _landmark.segment<2>(0) = Eigen::Vector2d::Random().normalized();
    _landmark.segment<3>(2) = Eigen::Vector3d::Random().normalized();
    LandMarkBlock landmark(1, _landmark);
    Eigen::Matrix<double, 3,5, RowMajor> _jacobian4 = Eigen::Matrix<double,3,5>::Constant(std::nan(""));
    // 
    parameters[0] = pose1.parameters();
    parameters[1] = pose2.parameters();
    parameters[2] = pose3.parameters();
    parameters[3] = landmark.parameters();

    jacobians[0] = _jacobian1.data();
    jacobians[1] = _jacobian2.data();
    jacobians[2] = _jacobian3.data();
    jacobians[3] = _jacobian4.data();
    
    EXPECT_NE(_transform1, _transform2);
    EXPECT_NE(_transform2, _transform3);
    // now we have random values for 3 edges...
    
    const double delta = 1e-08;
    const double relTol = 1e-6;
    // Our jacobians...

    pFactorNodes->Evaluate(parameters, residual.data(), jacobians);

    Eigen::Matrix<double, 3, 12, RowMajor> jacobian_delta;
    jacobian_delta.setZero();
    for (size_t i = 0; i < 6; ++i)
    {
        Eigen::Matrix<double, 6, 1> delta_pose = Eigen::Matrix<double, 6, 1>::Zero();
        delta_pose(i,0) = delta;
        Eigen::Matrix<double, 3, 4, RowMajor> _transform3_plus;
        PoseLocalParameterization pose_plus;
        pose_plus.Plus(pose3.parameters(), delta_pose.data(), _transform3_plus.data());
        EXPECT_NE(_transform3, _transform3_plus);
        
        Eigen::Vector3d residual_delta;
        PoseBlock pose3_plus(0, _transform3_plus);
        parameters[2] = pose3_plus.parameters();
        pFactorNodes->Evaluate(parameters, residual_delta.data(), NULL);
        jacobian_delta.col(i) = (residual_delta - residual) / delta;
    }

    Eigen::MatrixXd J_diff = jacobian_delta - _jacobian3;
    double maxDiff = std::max(-J_diff.minCoeff(), J_diff.maxCoeff());

    EXPECT_LE(maxDiff, relTol);
    delete[] jacobians;
}

TEST(Jacobian4Nodes_TEST /*test_case_name*/, Jacobian_LandMark /*test_name*/)
{
   Eigen::Vector3d measurement = Eigen::Vector3d::Random();
    Eigen::Matrix3d information = Eigen::Matrix3d::Identity();

    std::shared_ptr<CeresPBAFactor4Nodes> pFactorNodes(new Estimator::CeresPBAFactor4Nodes(measurement, information));

    // prepare the parameters and jacobian for test
    double **parameters, **jacobians;
    parameters = new double *[pFactorNodes->parameterBlocks()];
    jacobians = new double *[pFactorNodes->parameterBlocks()];

    Eigen::Vector3d residual;

    Eigen::Matrix<double, 3, 4, RowMajor> _transform1;
    _transform1.topLeftCorner<3,3>() = generateRandomRotation(3.0);
    _transform1.topRightCorner<3,1>() = Eigen::Vector3d::Random();
    PoseBlock pose1(0, _transform1);
    Eigen::Matrix<double, 3, 12, RowMajor> _jacobian1 = Eigen::Matrix<double,3,12>::Constant(std::nan(""));

    Eigen::Matrix<double, 3, 4, RowMajor> _transform2;
    _transform2.topLeftCorner<3,3>() = generateRandomRotation(3.0);
    _transform2.topRightCorner<3,1>() = Eigen::Vector3d::Random();
    PoseBlock pose2(1, _transform2);
    Eigen::Matrix<double, 3, 12, RowMajor> _jacobian2 = Eigen::Matrix<double,3,12>::Constant(std::nan(""));

     Eigen::Matrix<double, 3, 4, RowMajor> _transform3;
    _transform3.topLeftCorner<3,3>() = generateRandomRotation(3.0);
    _transform3.topRightCorner<3,1>() = Eigen::Vector3d::Random();
    PoseBlock pose3(2, _transform3);
    Eigen::Matrix<double, 3, 12, RowMajor> _jacobian3 = Eigen::Matrix<double,3,12>::Constant(std::nan(""));

    Eigen::Matrix<double, 5,1 > _landmark;
    _landmark.segment<2>(0) = Eigen::Vector2d::Random().normalized();
    _landmark.segment<3>(2) = Eigen::Vector3d::Random().normalized();
    LandMarkBlock landmark(1, _landmark);
    Eigen::Matrix<double, 3,5, RowMajor> _jacobian4 = Eigen::Matrix<double,3,5>::Constant(std::nan(""));
    // 
    parameters[0] = pose1.parameters();
    parameters[1] = pose2.parameters();
    parameters[2] = pose3.parameters();
    parameters[3] = landmark.parameters();

    jacobians[0] = _jacobian1.data();
    jacobians[1] = _jacobian2.data();
    jacobians[2] = _jacobian3.data();
    jacobians[3] = _jacobian4.data();
    
    EXPECT_NE(_transform1, _transform2);
    EXPECT_NE(_transform2, _transform3);
    // now we have random values for 3 edges...
    
    const double delta = 1e-08;
    const double relTol = 1e-6;
    // Our jacobians...

    pFactorNodes->Evaluate(parameters, residual.data(), jacobians);

    Eigen::Matrix<double, 3, 5, RowMajor> jacobian_delta;
    jacobian_delta.setZero();
    for (size_t i = 0; i < 1; ++i)
    {
        Eigen::Matrix<double, 1, 1> delta_pose = Eigen::Matrix<double, 1, 1>::Zero();
        delta_pose(i,0) = delta;
        Eigen::Matrix<double, 5, 1, ColMajor> _landmark_plus;
        LandMarkLocalParameterization pose_plus;
        pose_plus.Plus(landmark.parameters(), delta_pose.data(), _landmark_plus.data());
        EXPECT_NE(_landmark, _landmark_plus);
        
        Eigen::Vector3d residual_delta;
        LandMarkBlock landmark_plus(0, _landmark_plus);
        parameters[3] = landmark_plus.parameters();
        pFactorNodes->Evaluate(parameters, residual_delta.data(), NULL);
        jacobian_delta.col(i) = (residual_delta - residual) / delta;
    }

    Eigen::MatrixXd J_diff = jacobian_delta - _jacobian4;
    double maxDiff = std::max(-J_diff.minCoeff(), J_diff.maxCoeff());

    EXPECT_LE(maxDiff, relTol);
    delete[] jacobians;
}